// ==============================================================================
//
//   File...... GaitPIC-Quad.c
//   Purpose... QuadCrawler Gait Controller
//   Author.... (C) 2006 Steven R. Norris -- All Rights Reserved
// 		    For personal and non-commercial use only.
//   E-mail.... norris56@comcast.net
//   Started... 06/04/2006
//   Updated... 
//
// ==============================================================================

// ------------------------------------------------------------------------------
// Revision History
// ------------------------------------------------------------------------------

// 0622a - First version

#define CurrentVer "0622a"

// ------------------------------------------------------------------------------
// Includes
// ------------------------------------------------------------------------------
#include <16F688.h>
#include <stdlib.h>

// ------------------------------------------------------------------------------
// Directives
// ------------------------------------------------------------------------------
#fuses NOPROTECT,NOCPD,NOWDT,BROWNOUT,PUT,NOMCLR,INTRC_IO
#use delay(clock=8000000)
#use fast_io(A)
#use fast_io(C)
#use rs232(baud=2400,xmit=PIN_C4,rcv=PIN_C5,parity=N,bits=8,stream=BS2PSC)

// ------------------------------------------------------------------------------
// Function Templates
// ------------------------------------------------------------------------------

void Reset();

void Walk(int8 Gait);
void WalkHome(int8 LeftRamp, int8 RightRamp);
void WalkForward(int8 LeftRamp, int8 RightRamp);
void WalkBack(int8 LeftRamp, int8 RightRamp);
void WalkLeft(int8 LeftRamp, int8 RightRamp);
void WalkRight(int8 LeftRamp, int8 RightRamp);

void Send2PSC(int8 Servo, int8 Ramp, int16 ServoPos);

void ProcessSerialCmd();

char bgetc(void);
void GetString(char* s, int max);
void FlushRecvBuf();

void SaveOffsets();
void LoadOffsets();

// ------------------------------------------------------------------------------
// Definitions
// ------------------------------------------------------------------------------

// Pins
#define Pin_AllStop		PIN_C0
#define Pin_Busy		PIN_C1

// UART buffers
#define RX_BUFFER_SIZE	24

// Adjustable Ramp Values
#define LiftRamp		0x01			// Vertical servo ramp
#define VeryFast        0x08        	// Walk very fast
#define Fast            0x0A			// Walk fast

// Horizontal Leg Constants
#define Center          750				// Horizontal servo center value

// Stride Calculations
#define Stride          150				// Stride units
#define delay           80        		// in milliseconds

// Right side servos
#define Leg1Center      Center
#define Leg1Forward     Center+Stride
#define Leg1Back        Center-Stride

#define Leg2Center      Center
#define Leg2Forward     Center+Stride
#define Leg2Back        Center-Stride

// Left side servos
#define Leg3Center      Center
#define Leg3Forward     Center-Stride
#define Leg3Back        Center+Stride

#define Leg4Center      Center
#define Leg4Forward     Center-Stride
#define Leg4Back        Center+Stride

// Adjustable vertical servo positions
#define RaiseRight      300				// Raised and lowered
#define LowerRight      1200            // vertical servo values
#define RaiseLeft       1200
#define LowerLeft       300

// Gait tables
#define Table_Home		0
#define Table_Forward	1
#define Table_Back		2
#define Table_Left		3
#define Table_Right		4

// ------------------------------------------------------------------------------
// Constants
// ------------------------------------------------------------------------------

// Gait Tables
const int16 Home[25]    = {0x01,RaiseRight, 0x00,Center, 0x01,LowerRight,
        				   0x03,RaiseRight, 0x02,Center, 0x03,LowerRight,
        				   0x05,RaiseLeft,  0x04,Center, 0x05,LowerLeft,
        				   0x07,RaiseLeft,  0x06,Center, 0x07,LowerLeft,
					       0xFF};

const int16 Forward[33] = {0x01,RaiseRight, 0x07,RaiseLeft,
             			   0x00,Leg1Forward,0x02,Leg2Back,
             			   0x04,Leg3Center, 0x06,Leg4Center,
             			   0x01,LowerRight, 0x07,LowerLeft,
             			   0x03,RaiseRight, 0x05,RaiseLeft,
             			   0x00,Leg1Center, 0x02,Leg2Center,
             			   0x04,Leg3Forward,0x06,Leg4Back,
             			   0x03,LowerRight, 0x05,LowerLeft,
             			   0xFF };

const int16 Back[33]    = {0x01,RaiseRight, 0x07,RaiseLeft,
             			   0x00,Leg1Center, 0x02,Leg2Center,
             			   0x04,Leg3Forward,0x06,Leg4Back,
             			   0x01,LowerRight, 0x07,LowerLeft,
             			   0x03,RaiseRight, 0x05,RaiseLeft,
             			   0x00,Leg1Forward,0x02,Leg2Back,
             			   0x04,Leg3Center, 0x06,Leg4Center,
             			   0x03,LowerRight, 0x05,LowerLeft,
             			   0xFF};

const int16 Lturn[33]   = {0x01,RaiseRight, 0x07,RaiseLeft,
             			   0x00,Leg1Forward+Stride,0x06,Leg4Back+Stride,
             			   0x04,Leg3Forward,0x02,Leg2Back,
             			   0x01,LowerRight, 0x07,LowerLeft,
             			   0x03,RaiseRight, 0x05,RaiseLeft,
             			   0x00,Leg1Center+Stride,0x06,Leg4Center+Stride,
             			   0x04,Leg3Forward,0x02,Leg2Back,
             			   0x03,LowerRight, 0x05,LowerLeft,
             			   0xFF};

const int16 Rturn[33]   = {0x01,RaiseRight, 0x07,RaiseLeft,
             			   0x00,Leg1Forward,0x02,Leg2Center-Stride,
             			   0x04,Leg3Center-Stride,0x06,Leg4Back,
             			   0x01,LowerRight, 0x07,LowerLeft,
             			   0x03,RaiseRight, 0x05,RaiseLeft,
             			   0x00,Leg1Forward,0x02,Leg2Back-Stride,
             			   0x04,Leg3Forward-Stride,0x06,Leg4Back,
             			   0x03,LowerRight, 0x05,LowerLeft,
             			   0xFF};

// Gait translation table
const int8 GaitCodes[15] = {0x00,0x01,0x02,
							0x10,0x11,0x12,
							0x20,0x21,0x22,
							0x30,0x31,0x32,
							0x40,0x41,0x42};

// ------------------------------------------------------------------------------
// Global Variables
// ------------------------------------------------------------------------------

// Current walking command from BS2
int8 Gait = 0;
int8 GaitCount = 0;

signed int8 CenterOffset[4] = {0,0,0,0};

// UART Receive buffer
char Rx_Buffer[RX_BUFFER_SIZE+1]; 	// character array (buffer)
char RX_Wr_Index = 0; 				// index of next char to be put into the buffer
char RX_Rd_Index = 0; 				// index of next char to be fetched from the buffer
char RX_Counter = 0;  				// a total count of characters in the buffer
int1 RX_Buffer_Overflow = 0;   		// This flag is set on UART Receiver buffer overflow

// ------------------------------------------------------------------------------
// Interrupt Handlers
// ------------------------------------------------------------------------------

#int_rda
void serial_rx_isr()
{
    Rx_Buffer[RX_Wr_Index] = fgetc(BS2PSC);	// put received char in buffer 

    if(++RX_Wr_Index > RX_BUFFER_SIZE) 		// wrap the pointer 
        RX_Wr_Index = 0;

    if(++RX_Counter > RX_BUFFER_SIZE) 		// keep a character count 
    {                                 		// overflow check.. 
    	RX_Counter = RX_BUFFER_SIZE; 		// if too many chars came 
    	RX_Buffer_Overflow = 1;				// in before they could be used 
	}										// that could cause an error!! 
}

// ------------------------------------------------------------------------------
// Main Program
// ------------------------------------------------------------------------------

void main()
{
	// Setup PORT A
	setup_comparator(NC_NC_NC_NC);
	set_tris_a(0x08);
	output_a(0x00);

	// Setup PORT C
	set_tris_c(0x21);
	output_c(0x00);

	// Load center offsets from EEPROM
	LoadOffsets();

	// Enable interrupts
	enable_interrupts(INT_RDA);
	enable_interrupts(GLOBAL);

	#IGNORE_WARNINGS  203
	while(TRUE)
	{
		if(GaitCount > 0)
		{
			output_high(Pin_Busy);
			Walk(Gait);
			if(GaitCount != 0 && GaitCount != 0xFE)
				GaitCount--;
		}
		else
			output_low(Pin_Busy);

		ProcessSerialCmd();
	}
}

// ------------------------------------------------------------------------------
// Subroutines
// ------------------------------------------------------------------------------

void ProcessSerialCmd()
{
	char s[9];
	int8 leg;
	signed int16 ServoPos = 750;
	signed int8 offset;

	if(RX_Counter > 0)
	{
		GetString(s,8);
		if(s[0] == '!')
		{
			// Walk
			if(s[1] == 'w')
			{
				Gait = s[2];
				GaitCount = s[3];
			}
			// Reset
			else if(s[1] == 'r')
			{
				Reset();
			}
			// Set center offset
			else if(s[1] == 'c')
			{
				leg = s[2];
				switch(leg)
				{
					case 1:
						offset = s[3];
						CenterOffset[0] = offset;
						Send2PSC(0x00,Fast,ServoPos + offset);
						break;
					case 2:
						offset = s[3];
						CenterOffset[1] = offset;
						Send2PSC(0x02,Fast,ServoPos + offset);
						break;
					case 3:
						offset = s[3];
						CenterOffset[2] = offset;
						Send2PSC(0x04,Fast,ServoPos + offset);
						break;
					case 4:
						offset = s[3];
						CenterOffset[3] = offset;
						Send2PSC(0x06,Fast,ServoPos + offset);
						break;
					default:
						return;
				}
				SaveOffsets();
			}
		}
	}
}

void Reset()
{
	output_high(Pin_Busy);
	Gait = 0;
	GaitCount = 0;
	FlushRecvBuf();
	output_low(Pin_Busy);
}

void Walk(int8 GaitReq)
{
	int8 GaitCode;
	int8 GaitTable;
	int8 RightRamp;
	int8 LeftRamp;

	int8 GaitHigh;
	int8 GaitLow;
	int8 Ramp;

	// Emergency all stop
	if(input(Pin_AllStop))
	{
		Reset();
		return;
	}

	GaitCode = GaitCodes[GaitReq];
  	switch(GaitCode)
	{
    	case 0x00:						// Home
      		GaitTable = Table_Home;
      		RightRamp = Fast;
      		LeftRamp = Fast;
			break;
    	case 0x01:						// Spin left
      		GaitTable = Table_Left;
      		RightRamp = VeryFast;
      		LeftRamp = VeryFast;
			break;
    	case 0x02:						// Spin right
      		GaitTable = Table_Right;
      		rightRamp = VeryFast;
      		leftRamp = VeryFast;
			break;

		default:
			GaitHigh = (GaitCode & 0xF0) >> 4;
			GaitLow = (GaitCode & 0x0F);

			// Assign direction
		  	if(GaitHigh <= 0x02)
		    	GaitTable = Table_Forward;
		  	else
		    	GaitTable = Table_Back;
		
			// Assign ramp
		  	if(GaitHigh == 0x01 || GaitHigh == 0x04)
		    	Ramp = VeryFast;
		  	else
		    	Ramp = Fast;
		
			// Assign left/right leg speed
		  	switch(GaitLow)
			{
		    	case 0x00:
					// Straight
		      		RightRamp = Ramp;
		      		LeftRamp = Ramp;
					break;
		    	case 0x01:
					// Left
		      		LeftRamp = Ramp + 2;
		      		RightRamp = Ramp;
					break;
		    	case 0x02:
					// Right
		      		LeftRamp = Ramp;
		      		RightRamp = Ramp + 2;
					break;
			}
			break;
	}

	// Select walking engine
	switch(GaitTable)
	{
		case Table_Home:
			WalkHome(LeftRamp,RightRamp);
			break;

		case Table_Forward:
			WalkForward(LeftRamp,RightRamp);
			break;

		case Table_Back:
			WalkBack(LeftRamp,RightRamp);
			break;

		case Table_Left:
			WalkLeft(LeftRamp,RightRamp);
			break;

		case Table_Right:
			WalkRight(LeftRamp,RightRamp);
			break;
	}
}

// Walking Engines
void WalkHome(int8 LeftRamp, int8 RightRamp)
{
	int8 idx = 0;
	int8 Servo = 0;
	signed int16 ServoPos;
	int8 Ramp;
	int8 d = 0;

	Servo = make8(Home[idx],0);
  	while(Servo != 0xFF)
	{
		// Emergency all stop
		if(input(Pin_AllStop))
		{
			Reset();
			return;
		}

		ServoPos = Home[idx+1];
    	if((Servo % 2) == 1)
		{
			// Vertical servo
      		Ramp = LiftRamp;
		}
    	else
		{
			// Horizontal servo
			switch(Servo)
			{
				case 0:
					ServoPos += CenterOffset[0];
	        		Ramp = RightRamp;
					break;
				case 2:
					ServoPos += CenterOffset[1];
	        		Ramp = RightRamp;
					break;
				case 4:
					ServoPos += CenterOffset[2];
	        		Ramp = LeftRamp;
					break;
				case 6:
					ServoPos += CenterOffset[3];
	        		Ramp = LeftRamp;
					break;
			}
		}
		Send2PSC(Servo,Ramp,ServoPos);

		// Delay every 3rd servo command
		d++;
		if(d == 3)
		{
    		delay_ms(delay);
			d = 0;
		}

    	idx += 2;
		Servo = make8(Home[idx],0);
	}
}

void WalkForward(int8 LeftRamp, int8 RightRamp)
{
	int8 idx = 0;
	int8 Servo = 0;
	int16 ServoPos;
	int8 Ramp;
	int8 d = 0;

	Servo = make8(Forward[idx],0);
  	while(Servo != 0xFF)
	{
		// Emergency all stop
		if(input(Pin_AllStop))
		{
			Reset();
			return;
		}

		ServoPos = Forward[idx+1];
    	if((Servo % 2) == 1)
		{
			// Vertical servo
      		Ramp = LiftRamp;
		}
    	else
		{
			// Horizontal servo
			switch(Servo)
			{
				case 0:
					ServoPos += CenterOffset[0];
	        		Ramp = RightRamp;
					break;
				case 2:
					ServoPos += CenterOffset[1];
	        		Ramp = RightRamp;
					break;
				case 4:
					ServoPos += CenterOffset[2];
	        		Ramp = LeftRamp;
					break;
				case 6:
					ServoPos += CenterOffset[3];
	        		Ramp = LeftRamp;
					break;
			}
		}
		Send2PSC(Servo,Ramp,ServoPos);

		// Delay every 2nd servo command
		d++;
		if(d == 2)
		{
    		delay_ms(delay);
			d = 0;
		}

    	idx += 2;
		Servo = make8(Forward[idx],0);
	}

}

void WalkBack(int8 LeftRamp, int8 RightRamp)
{
	int8 idx = 0;
	int8 Servo = 0;
	int16 ServoPos;
	int8 Ramp;
	int8 d = 0;

	Servo = make8(Back[idx],0);
  	while(Servo != 0xFF)
	{
		// Emergency all stop
		if(input(Pin_AllStop))
		{
			Reset();
			return;
		}

		ServoPos = Back[idx+1];
    	if((Servo % 2) == 1)
		{
			// Vertical servo
      		Ramp = LiftRamp;
		}
    	else
		{
			// Horizontal servo
			switch(Servo)
			{
				case 0:
					ServoPos += CenterOffset[0];
	        		Ramp = RightRamp;
					break;
				case 2:
					ServoPos += CenterOffset[1];
	        		Ramp = RightRamp;
					break;
				case 4:
					ServoPos += CenterOffset[2];
	        		Ramp = LeftRamp;
					break;
				case 6:
					ServoPos += CenterOffset[3];
	        		Ramp = LeftRamp;
					break;
			}
		}
		Send2PSC(Servo,Ramp,ServoPos);

		// Delay every 2nd servo command
		d++;
		if(d == 2)
		{
    		delay_ms(delay);
			d = 0;
		}

    	idx += 2;
		Servo = make8(Back[idx],0);
	}
}

void WalkLeft(int8 LeftRamp, int8 RightRamp)
{
	int8 idx = 0;
	int8 Servo = 0;
	int16 ServoPos;
	int8 Ramp;
	int8 d = 0;

	Servo = make8(Lturn[idx],0);
  	while(Servo != 0xFF)
	{
		// Emergency all stop
		if(input(Pin_AllStop))
		{
			Reset();
			return;
		}

		ServoPos = Lturn[idx+1];
    	if((Servo % 2) == 1)
		{
			// Vertical servo
      		Ramp = LiftRamp;
		}
    	else
		{
			// Horizontal servo
			switch(Servo)
			{
				case 0:
					ServoPos += CenterOffset[0];
	        		Ramp = RightRamp;
					break;
				case 2:
					ServoPos += CenterOffset[1];
	        		Ramp = RightRamp;
					break;
				case 4:
					ServoPos += CenterOffset[2];
	        		Ramp = LeftRamp;
					break;
				case 6:
					ServoPos += CenterOffset[3];
	        		Ramp = LeftRamp;
					break;
			}
		}
		Send2PSC(Servo,Ramp,ServoPos);

		// Delay every 2nd servo command
		d++;
		if(d == 2)
		{
    		delay_ms(delay);
			d = 0;
		}

    	idx += 2;
		Servo = make8(Lturn[idx],0);
	}
}

void WalkRight(int8 LeftRamp, int8 RightRamp)
{
	int8 idx = 0;
	int8 Servo = 0;
	int16 ServoPos;
	int8 Ramp;
	int8 d = 0;

	Servo = make8(Rturn[idx],0);
  	while(Servo != 0xFF)
	{
		// Emergency all stop
		if(input(Pin_AllStop))
		{
			Reset();
			return;
		}

		ServoPos = Rturn[idx+1];
    	if((Servo % 2) == 1)
		{
			// Vertical servo
      		Ramp = LiftRamp;
		}
    	else
		{
			// Horizontal servo
			switch(Servo)
			{
				case 0:
					ServoPos += CenterOffset[0];
	        		Ramp = RightRamp;
					break;
				case 2:
					ServoPos += CenterOffset[1];
	        		Ramp = RightRamp;
					break;
				case 4:
					ServoPos += CenterOffset[2];
	        		Ramp = LeftRamp;
					break;
				case 6:
					ServoPos += CenterOffset[3];
	        		Ramp = LeftRamp;
					break;
			}
		}
		Send2PSC(Servo,Ramp,ServoPos);

		// Delay every 2nd servo command
		d++;
		if(d == 2)
		{
    		delay_ms(delay);
			d = 0;
		}

    	idx += 2;
		Servo = make8(Rturn[idx],0);
	}
}

void Send2PSC(int8 Servo, int8 Ramp, int16 ServoPos)
{
	fprintf(BS2PSC,"!SC");
	fputc(Servo,BS2PSC);
	fputc(Ramp,BS2PSC);
	fputc(make8(ServoPos,0),BS2PSC);
	fputc(make8(ServoPos,1),BS2PSC);
	fputc(0x0d,BS2PSC);
}

void GetString(char* s, int max)
{
	int len;
   	char c;

   	--max;
   	len = 0;
   	do
	{
    	c = bgetc();
   		if(len < max)
	   		s[len++] = c;
   	} while(c != 0xFF);
   	s[len] = 0;
}

char bgetc(void)
{
	char c;

	while(RX_Counter == 0);					// wait for a character... 

	c = Rx_Buffer[RX_Rd_Index]; 			// get one from the buffer..

    if(++RX_Rd_Index > RX_BUFFER_SIZE) 		// wrap the pointer 
        RX_Rd_Index = 0;

	if(RX_Counter)
		RX_Counter--;        				// keep a count (buffer size) 

	return c;
}

void FlushRecvBuf()
{
	RX_Wr_Index = 0;
	RX_Rd_Index = 0;
	RX_Counter = 0;
	RX_Buffer_Overflow = 0;
}

void SaveOffsets()
{
	int8 i;
	for(i = 0; i < 4; i++)
		write_eeprom(i,CenterOffset[i]);
}

void LoadOffsets()
{
	int8 i;
	for(i = 0; i < 4; i++)
		CenterOffset[i] = read_eeprom(i);
}

